#include <QtGui/QPen>
#include <QDomDocument>
#include <QDomNodeList>
#include <QDomElement>
#include <QDebug>
#include <QFile>
#include <qmath.h>
#include <fstream>
#include <string>

#include "streetmap.h"
#include "line.h"
#include "linesegment.h"
#include "utilities.h"
#include "geometryutilities.h"

//-----------------------------------------------------------------------------
// StreetMap::Road
//-----------------------------------------------------------------------------
StreetMap::Road::Road()
: mStreet("NO STREET"), mSpeedLimit(0), mIFirst(NULL), mISecond(NULL)
{
}

StreetMap::Road::Road(unsigned int speedLimit, const QString& streetName,
                      StreetMap::Intersection* first, StreetMap::Intersection* second)
:  mStreet(streetName), mSpeedLimit(speedLimit), mFirst(first->location()),
   mSecond(second->location()), mIFirst(first), mISecond(second)
{
}

qreal StreetMap::Road::length() const
{
  return (mFirst - mSecond).magnitude();
}

const QString& StreetMap::Road::streetName() const
{
  return mStreet;
}

void StreetMap::Road::setStreetName(const QString& newName)
{
  mStreet = newName;
}

unsigned int StreetMap::Road::speedLimit() const
{
  return mSpeedLimit;
}


const GeometryUtilities::Point& StreetMap::Road::first() const
{
  return mFirst;
}

const GeometryUtilities::Point& StreetMap::Road::second() const
{
  return mSecond;
}

GeometryUtilities::Point& StreetMap::Road::first()
{
  return mFirst;
}

GeometryUtilities::Point& StreetMap::Road::second()
{
  return mSecond;
}

const StreetMap::Intersection *StreetMap::Road::getFirstIntersection() const
{
  return mIFirst;
}

const StreetMap::Intersection *StreetMap::Road::getSecondIntersection() const
{
  return mISecond;
}

StreetMap::Intersection *StreetMap::Road::getFirstIntersection()
{
  return mIFirst;
}

StreetMap::Intersection *StreetMap::Road::getSecondIntersection()
{
  return mISecond;
}

unsigned int StreetMap::Road::width() const
{
  return (mFirstToSecondLanes.count()+mSecondToFirstLanes.count())*LANE_WIDTH;
}

unsigned int StreetMap::Road::numLanes(StreetMap::Road::Lane::DIRECTION d) const
{
  return d==Lane::FROM_FIRST_TO_SECOND?mFirstToSecondLanes.count():mSecondToFirstLanes.count();
}

unsigned int StreetMap::Road::numLanes() const
{
  return mFirstToSecondLanes.count()+mSecondToFirstLanes.count();
}

const StreetMap::Road::Lane& StreetMap::Road::lane(StreetMap::Road::Lane::DIRECTION d, int lanePos, bool& valid) const
{
  const QList<Lane> &lanes = d==Lane::FROM_FIRST_TO_SECOND?mFirstToSecondLanes:mSecondToFirstLanes;
  if (lanePos < 0 || lanePos > lanes.count())
  {
    valid = false;
    return lanes.first();
  }
  else
  {
    valid = true;
    return lanes.at(lanePos);
  }
}
/*
int StreetMap::Road::laneTurnType(StreetMap::Road::Lane::DIRECTION d, int lanePos) const
{
  const QList<Lane> &lanes = d==Lane::FROM_FIRST_TO_SECOND?mFirstToSecondLanes:mSecondToFirstLanes;
  if (lanePos < 0 || lanePos > lanes.count()) return Lane::NO_TYPE;
  else return lanes.at(lanePos).type;
}
*/
void StreetMap::Road::addLane(int lanePos, StreetMap::Road::Lane::TURN_TYPE type,
                              StreetMap::Road::Lane::DIRECTION dir,
                              bool replace)
{
  QList<Lane>& lanes = (dir==Lane::FROM_FIRST_TO_SECOND?mFirstToSecondLanes:mSecondToFirstLanes);
  if (lanePos >= 0 && lanes.count() > lanePos && replace)
  {
    Lane lane = {dir, type};
    lanes.replace(lanePos, lane);
  }
  else if (lanePos >= 0)
  {
    Lane actual = {dir, type},
    temp = {dir, Lane::STRAIGHT};
    while (lanes.count()+1 < lanePos) // +1 to handle zero-indexing.
      lanes.push_back(temp);
    lanes.push_back(actual);
  }
  recalculateLaneSegments();
}

void StreetMap::Road::addLane(const StreetMap::Road::Lane& l)
{
  if (l.direction == Lane::FROM_FIRST_TO_SECOND) mFirstToSecondLanes.push_back(l);
  else if (l.direction == Lane::FROM_SECOND_TO_FIRST) mSecondToFirstLanes.push_back(l);
  recalculateLaneSegments();
}

void StreetMap::Road::recalculateLaneSegments()
{
  GeometryUtilities::LineSegment directLine(mFirst, mSecond);

  GeometryUtilities::LineSegment sideLine =
      directLine.toOffset(width()/2);
  int laneCount = 0;

  for (int i=mSecondToFirstLanes.size()-1; i>=0; i--)
  {
    mSecondToFirstLanes[i].segment = sideLine.toOffset(laneCount * -LANE_WIDTH -(LANE_WIDTH/2));
    laneCount++;
  }

  for (int i=0; i<mFirstToSecondLanes.size(); i++)
  {
    mFirstToSecondLanes[i].segment = sideLine.toOffset(laneCount * -LANE_WIDTH -(LANE_WIDTH/2));
    laneCount++;
  }

}

StreetMap::Road::Lane::DIRECTION
    StreetMap::Road::getDirection(const Intersection *goingTowards) const
{
  if (goingTowards==NULL)
    return Lane::NO_LANE;
  if (goingTowards == getFirstIntersection())
    return Lane::FROM_FIRST_TO_SECOND;
    //return Lane::FROM_SECOND_TO_FIRST;
  else if (goingTowards == getSecondIntersection())
    return Lane::FROM_SECOND_TO_FIRST;
    //return Lane::FROM_FIRST_TO_SECOND;
  return Lane::NO_LANE;
}

//-----------------------------------------------------------------------------
// StreetMap::Intersection
//-----------------------------------------------------------------------------
StreetMap::Intersection::Intersection()
: mLocation(0.0,0.0) {}

StreetMap::Intersection::Intersection(const QString& name, const GeometryUtilities::Point& location)
: mName(name), mLocation(location) {}

const GeometryUtilities::Point& StreetMap::Intersection::location() const {return mLocation;}
const QString& StreetMap::Intersection::name() const {return mName;}

void StreetMap::Intersection::addNeighbor(StreetMap::Intersection* neighbor)
{
  mNeighbors.push_back(neighbor);
}


//-----------------------------------------------------------------------------
// StreetMap
//-----------------------------------------------------------------------------
StreetMap::StreetMap(){}

StreetMap::StreetMap(const QString &fileName, const QSize &size)
{
  QDomDocument doc( "Scenario" );
  QFile file( fileName );
  if( !file.open( QIODevice::ReadOnly ) )
  {
    qDebug() << "XML: ERROR could not open map XML>" << fileName;
    return;
  }
  if( !doc.setContent( &file  ) )
  {
    qDebug() << "XML: ERROR could not read map XML (malformed?)>" << fileName;
    file.close();
    return;
  }
  file.close();


  QDomNodeList intersections = doc.elementsByTagName("Intersection");
  for (int n=0; n<intersections.size(); ++n)
  {
    QDomElement intersection = intersections.at(n).toElement();
    qreal x = intersection.attribute("x").toFloat(), // * size.width(),
          y = intersection.attribute("y").toFloat(); // * size.height();
    QString name = intersection.attribute("name");
    Intersection *i = new Intersection(name, GeometryUtilities::Point(x,y));
    if (mIntersections.contains(name))
      delete mIntersections[name];
    mIntersections[name] = i;
  }

  QDomNodeList roads = doc.elementsByTagName("Road");
  for (int n=0; n<roads.count(); ++n)
  {
    QDomElement road = roads.at(n).toElement();
    Road *r = new Road(road.attribute("speedLimit").toUInt(),
                       road.attribute("streetName"),
                       mIntersections[road.attribute("begin")],
                       mIntersections[road.attribute("end")]);
    QDomNodeList lanes = road.elementsByTagName("Lane");
    for (int j=0; j<lanes.count(); ++j)
    {
      QDomElement lane = lanes.at(j).toElement();
      Road::Lane l;
      l.type = Road::Lane::NO_TYPE;
      QString dirStr = lane.attribute("dir").toUpper(), typeStr = lane.attribute("type").toUpper();
      if (dirStr == "FTOS") l.direction = Road::Lane::FROM_FIRST_TO_SECOND;
      else if (dirStr == "STOF") l.direction = Road::Lane::FROM_SECOND_TO_FIRST;
      else l.direction = Road::Lane::BOTH;
      if (typeStr.contains("S")) l.type |= Road::Lane::STRAIGHT;
      if (typeStr.contains("L")) l.type |= Road::Lane::LEFT;
      if (typeStr.contains("R")) l.type |= Road::Lane::RIGHT;
      r->addLane(l);
      qDebug() << r->getFirstIntersection()->name() << "<->" << r->getSecondIntersection()->name() << " has lane of type " << l.type << " and dir " << l.direction;
    }
    mRoads.push_back(r);
  }

  createIntersectionGaps();
}

StreetMap::~StreetMap()
{
  foreach(Road *r, mRoads)
    delete r;
  foreach(Intersection *i, mIntersections)
    delete i;

  mRoads.clear();
  mIntersections.clear();
}


const StreetMap::Intersection* StreetMap::getIntersection(const GeometryUtilities::Point& point) const
{
  Intersection *ret = NULL;
  foreach(Intersection *i, mIntersections)
  {
    if (ret == NULL)
    {
      ret = i;
    }
    else if ((i->location()-point).magnitude() < (ret->location()-point).magnitude())
    {
      ret = i;
    }
  }
  return ret;
}

StreetMap::Intersection* StreetMap::getIntersection(const GeometryUtilities::Point& point)
{
  Intersection *ret = NULL;
  foreach(Intersection *i, mIntersections)
  {
    if (ret == NULL)
    {
      ret = i;
    }
    else if ((i->location()-point).magnitude() < (ret->location()-point).magnitude())
    {
      ret = i;
    }
  }
  return ret;
}

const StreetMap::Intersection* StreetMap::getIntersection(const QString& name) const
{
  if (mIntersections.contains(name)) return mIntersections[name];
  else return NULL;
}

const StreetMap::Road* StreetMap::getRoad(const GeometryUtilities::Point &point) const
{
  Road *ret = NULL;
  foreach(Road* r, mRoads)
  {
    if (ret == NULL)
    {
      ret = r;
    }
    else
    {
      GeometryUtilities::Point p1 = (r->second()+r->first())/2.0,
                               p2 = (ret->second()+ret->first())/2.0;
      if ((p1-point).magnitude() < (p2-point).magnitude())
        ret = r;
    }
  }

  return ret;
}

const StreetMap::Road* StreetMap::getRoad(const QString &roadName) const
{
  foreach(Road* r, mRoads)
  {
    if (r && r->streetName() == roadName)
      return r;
  }
  return NULL;
}

const StreetMap::Intersection* StreetMap::getIntersection(const StreetMap::Intersection *previousIntersection,
                                                          const StreetMap::Road *currentRoad) const
{
  if (currentRoad->getFirstIntersection() == previousIntersection)
  {
    return currentRoad->getSecondIntersection();
  }
  else
  {
    return currentRoad->getFirstIntersection();
  }
}

const StreetMap::Road* StreetMap::getRoad(
    const StreetMap::Intersection* intersection1,
    const StreetMap::Intersection* intersection2) const
{
  if (!intersection1 || !intersection2 )
    return NULL;
  foreach (Road* r, mRoads)
  {
    if (
      ( (r->getFirstIntersection() && r->getFirstIntersection()->name() == intersection1->name())
         && (r->getSecondIntersection() && r->getSecondIntersection()->name() == intersection2->name()) )
      ||
      ( (r->getFirstIntersection() && r->getFirstIntersection()->name() == intersection2->name())
         && (r->getSecondIntersection() && r->getSecondIntersection()->name() == intersection1->name()) )
      )
      return r;
  }
  return NULL;
}

StreetMap::RoadList StreetMap::getRoads(const StreetMap::Intersection *intersection) const
{
  RoadList roads;

  if (intersection)
  {
    foreach (Road* r, mRoads)
    {
      if (r->getFirstIntersection() && r->getFirstIntersection()->name() == intersection->name())
      {
        roads.append(r);
      }

      if (r->getSecondIntersection() && r->getSecondIntersection()->name() == intersection->name())
      {
        roads.append(r);
      }
    }
  }

  return roads;
}

StreetMap::RoadList StreetMap::getRoads() const
{
  return mRoads;
}

StreetMap::IntersectionList StreetMap::getIntersections() const
{
  return mIntersections.values();
}

void StreetMap::createIntersectionGaps()
{
  qDebug() << "createIntersectionGaps";
  foreach(Intersection *i, mIntersections)
  {
    RoadList roads = getRoads(i);
    RoadList::iterator outerRoadItr;
    for (outerRoadItr=roads.begin(); outerRoadItr!=roads.end(); ++outerRoadItr)
    {
      RoadList::iterator innerRoadItr;
      for (innerRoadItr=roads.begin(); innerRoadItr!=roads.end();++innerRoadItr)
      {
        Road *outerRoad = *outerRoadItr,
             *innerRoad = *innerRoadItr;
        //Generate the line segments from these roads to do collision on.
        qreal ow = outerRoad->width(),
              iw = innerRoad->width();
        GeometryUtilities::LineSegment oroad(outerRoad->first(), outerRoad->second()),
                                       iroad(innerRoad->first(), innerRoad->second());
        GeometryUtilities::LineSegment op(oroad.toOffset((ow/2))), on(oroad.toOffset(-(ow/2))),
                                       ip(iroad.toOffset((iw/2))), in(iroad.toOffset(-(iw/2)));
        GeometryUtilities::IntersectionInfo opipi, opini, onipi, onini;
        GeometryUtilities::Point
            opip(GeometryUtilities::LineSegmentLineSegmentIntersect(op,ip,opipi)),
            opin(GeometryUtilities::LineSegmentLineSegmentIntersect(op,in,opini)),
            onip(GeometryUtilities::LineSegmentLineSegmentIntersect(on,ip,onipi)),
            onin(GeometryUtilities::LineSegmentLineSegmentIntersect(on,in,onini));
        // Go through each intersection (ugly code here)
        if (opipi == GeometryUtilities::INTERSECTION)
        {
          bool ob=outerRoad->getFirstIntersection()->name() == i->name(),
               ib=innerRoad->getFirstIntersection()->name() == i->name();
          GeometryUtilities::Point ooff(ob?op.begin()-opip:op.end()-opip),
                                   ioff(ib?ip.begin()-opip:ip.end()-opip);
          (ob?outerRoad->first():outerRoad->second()) -= ooff;
          (ib?innerRoad->first():innerRoad->second()) -= ioff;
        }
        if (opini == GeometryUtilities::INTERSECTION)
        {
          bool ob=outerRoad->getFirstIntersection()->name() == i->name(),
               ib=innerRoad->getFirstIntersection()->name() == i->name();
          GeometryUtilities::Point ooff(ob?op.begin()-opin:op.end()-opin),
                                   ioff(ib?in.begin()-opin:in.end()-opin);
          (ob?outerRoad->first():outerRoad->second()) -= ooff;
          (ib?innerRoad->first():innerRoad->second()) -= ioff;
        }
        if (onipi == GeometryUtilities::INTERSECTION)
        {
          bool ob=outerRoad->getFirstIntersection()->name() == i->name(),
               ib=innerRoad->getFirstIntersection()->name() == i->name();
          GeometryUtilities::Point ooff(ob?on.begin()-onip:on.end()-onip),
                                   ioff(ib?ip.begin()-onip:ip.end()-onip);
          (ob?outerRoad->first():outerRoad->second()) -= ooff;
          (ib?innerRoad->first():innerRoad->second()) -= ioff;
        }
        if (onini == GeometryUtilities::INTERSECTION)
        {
          bool ob=outerRoad->getFirstIntersection()->name() == i->name(),
               ib=innerRoad->getFirstIntersection()->name() == i->name();
          GeometryUtilities::Point ooff(ob?on.begin()-onin:on.end()-onin),
                                   ioff(ib?in.begin()-onin:in.end()-onin);
          (ob?outerRoad->first():outerRoad->second()) -= ooff;
          (ib?innerRoad->first():innerRoad->second()) -= ioff;
        }

        innerRoad->recalculateLaneSegments();
        outerRoad->recalculateLaneSegments();
      }

    }
  }
}
